#include "Triangulator.h"
#include "Edge.h"

Triangulator::Triangulator(int width, int height, vector<Point*> points){
        // Maria did not comment her code - This is Jonathan

        // Make a new 4-ary priority heap queue
	triangles = new HeapPQueue<Triangle*, TriangleCompare>(4);
	numberOfPoints = points.size();
        // We will start with 4 triangles after initialization
	numOfTriangles = 4;
	initializeBoundingRectangle(width, height);

        numFlips = 1;
	
	for (int i = 0; i< numberOfPoints; i++){
		addPoint(points[i]);
                // Whenever you make a new triangle inside a triangle, you end up
                // with 2 more than you started with
		numOfTriangles = 2 + numOfTriangles;
                //flip();
	}
	//flip();
}

Triangulator::~Triangulator(){
	delete triangles;
}

vector<Triangle*> Triangulator::getTriangles() {
        // Jonathan left to get donuts.
        // Initialize a vector of Triangle* of appropriate size.
        // Awesome ensues.
	vector<Triangle*>* ts = new vector<Triangle*>(numOfTriangles);
	int i = 0;
	while (!triangles->isEmpty()){
		triangles->deleteMin(ts->at(i));
		if (ts->at(i) != NULL){
			i++;
		}
	}
	ts->resize(i);

        // We now pick a starting room and exit
        srand( time(NULL) );

        int start = (int) (rand() % ts->size() );
        int exit = (int) (rand() % ts->size() );

        while (start == exit) {
            exit = (int) (rand() % ts->size() );
        }

        ts->at(start)->entrance = true;
        ts->at(exit)->exit = true;

	return *ts;
}

void Triangulator::addPoint(Point* newPoint){
	if (!pointIsValid(newPoint) || triangles->isEmpty()){
		return; //silently fail, need a better solution
	}
	
	Triangle* oldTriangle;
	
	/*First, we need to find the smallest triangle that contains newPoint
	 The way we do this (inefficient) is have a priority queue that orders the triangles by size
	 Then pop the triangles one by one until we find one that contains newPoint
	 */
	triangles->deleteMin(oldTriangle);
	
	if (contains(newPoint, oldTriangle)){
		//This is the triangle we want. 
		//Carefully construct three smaller triangles, and reset their neighbour pointers
		Triangle* t1 = new Triangle(oldTriangle->getPoint1(), oldTriangle->getPoint2(), newPoint);
		Triangle* neighbour = oldTriangle->getNeighbourBetween12();
		if (neighbour != NULL && neighbour->assignNeighbour(t1)){
			t1->setNeighbourBetween12(neighbour);
		}
		
		Triangle* t2 = new Triangle(newPoint, oldTriangle->getPoint3(), oldTriangle->getPoint1());
		neighbour = oldTriangle->getNeighbourBetween31();
		if (neighbour != NULL && neighbour->assignNeighbour(t2)){
			t2->setNeighbourBetween23(neighbour);
		}
	
		Triangle* t3 = new Triangle(oldTriangle->getPoint2(), newPoint, oldTriangle->getPoint3());
		neighbour = oldTriangle->getNeighbourBetween23();
		if (neighbour != NULL && neighbour->assignNeighbour(t3)){
			t3->setNeighbourBetween31(neighbour);
		}
		
		t1->setNeighbourBetween23(t3);
		t1->setNeighbourBetween31(t2);
		
		t2->setNeighbourBetween31(t1);
		t2->setNeighbourBetween12(t3);
		
		t3->setNeighbourBetween12(t1);
		t3->setNeighbourBetween23(t2);

		triangles->insert(t1);
		triangles->insert(t2);
		triangles->insert(t3);
		delete oldTriangle;
	} else {
		//So newPoint is not in this triangle, try again, then add the oldTriangle back in
		addPoint(newPoint);
		triangles->insert(oldTriangle);
	}
}

bool Triangulator::contains(Point* p, Triangle* t){
	Point* p1 = t->getPoint1();
	Point* p2 = t->getPoint2();
	Point* p3 = t->getPoint3();
	
	int isCcw = ccw(p1, p2, p);
	return (isCcw == ccw(p2, p3, p))&&(isCcw == ccw(p3, p1, p));
}

int Triangulator::ccw(Point* p1, Point* p2, Point* p3){
	int dx1, dx2, dy1, dy2;
	
	dx1 = p2->getX() - p1->getX(); 
	dy1 = p2->getY() - p1->getY();
	dx2 = p3->getX() - p1->getX(); 
	dy2 = p3->getY() - p1->getY();
	
	if (dx1*dy2 > dy1*dx2)
		return +1;
	if (dx1*dy2 < dy1*dx2)
		return -1;
	if ((dx1*dx2 < 0) || (dy1*dy2 < 0))
		return -1;
	if ((dx1*dx1 + dy1*dy1) < (dx2*dx2 + dy2*dy2))
		return +1;
	
	return 0;
}

bool Triangulator::pointIsValid(Point* p){
	if (p == NULL){
		return false;
	}
	
	return p->getX()<width && p->getX()>0 && p->getY()<height && p->getY()>0;
}

/*Create points on the four corners of the bounding rectangle
 Draw a diagonal line across the point at (0, 0) and (width, height)
 by creating two new triangles and inserting them into the heap
 Returns false if dimension is invalid
 */
bool Triangulator::initializeBoundingRectangle(int width, int height){
	if (width < 2 || height < 2){
		return false;
	}
	
	this->width = width;
	this->height = height;
	
	Point* p1 = new Point (0, 0);
	Point* p2 = new Point (0, height);
	Point* p3 = new Point (width, height);
	Point* p4 = new Point (width, 0);
        Point* p5 = new Point ( (int)(width/2), (int)(height/2));
	
	Triangle* t1 = new Triangle(p1, p2, p5);
	Triangle* t2 = new Triangle(p1, p5, p4);
        Triangle* t3 = new Triangle(p4, p5, p3);
        Triangle* t4 = new Triangle(p3, p5, p2);
	
	t1->setNeighbourBetween23(t4);
        t1->setNeighbourBetween31(t2);

	t2->setNeighbourBetween12(t1);
        t2->setNeighbourBetween23(t3);

        t3->setNeighbourBetween12(t2);
        t3->setNeighbourBetween23(t4);

        t4->setNeighbourBetween12(t3);
        t4->setNeighbourBetween23(t1);

	
	triangles->insert(t1);
	triangles->insert(t2);
        triangles->insert(t3);
        triangles->insert(t4);
	
	return true;
}

//*****************************************************************************************************


// make sure that the points (p1,p2,p3) of a triangle are in counter-clockwise orientation
void Triangulator::turnccw()
{
	// assign all the elements of triangles (the heap queue) to a new vector, tri
	// after getTriangles() is called, triangles (the heap queue) would be empty while 
	// tri would contain all triangles that are originally in the heap queue
	int i;
	vector <Triangle*> ts = copyTriangles();
	triangles->makeEmpty();
	int size = ts.size();
	for (i = 0; i < size; i++)
	{
		Triangle *t = ts[i];
		// for each triangle, if p1,p2,p3 are in clcokwise orientation
		// swap p1 an p2, so that (p1,p2,p3) are in counterclockwise oritentation
		if(ccw(t->getPoint1(),t->getPoint2(),t->getPoint3()) == -1)
		{
			Point *temp = t->getPoint1();
			t->setPoint1(t->getPoint2());
			t->setPoint2(temp);
		}
		// insert every element of tri back to triangles (the heap queue)
		// after changes to (p1,p2,p3) has been made if neccessary 
		triangles->insert(t);
	}
}

// iterate every triangles in the heap queue
// flip the triangles if neccessary
// update the heap queue so every triangle is Delaunany with respect to all its neighbours
void Triangulator::flip()
{
	// first make sure (p1,p2,p3) are in counterclockwise orientation to each other
	// otherwise, it might cause error when determining if a triangle is Delaunay with respect to its neighbours
	turnccw();
        numFlips++;

	// assign all the elements of triangles (the heap queue) to a new vector, tri
	// after getTriangles() is called, triangles (the heap queue) would be empty while 
	// tri would contain all triangles that are originally in the heap queue
	int i;
	vector <Triangle*> ts = copyTriangles();
	int size = ts.size();
	for (i = 0; i < size; i++)
	{
		Triangle * t = ts[i];
		Triangle *n1 = t->getNeighbourBetween12();
		Triangle *n2 = t->getNeighbourBetween23();
		Triangle *n3 = t->getNeighbourBetween31();
		int k1 = -1, k2 = -1, k3 = -1;
		for (int j = 0; j < size; j++)
		{
			if (n1 != NULL && isTheSameTriangle(n1,ts[j]))
			{
				k1 = j;
			}
			if (n2 != NULL && isTheSameTriangle(n2,ts[j]))
			{
				k2 = j;
			}
			if (n3 != NULL && isTheSameTriangle(n3,ts[j]))
			{
				k3 = j;
			}			
		}

		// assumption: for every triangle, at most one of its neighbour is not Delaunany
		// with respect to the triangle
		// if we iterate through all the triangles, this would check whether every triangle
		// is Delaunay with respect to all its 3 neighbours 
		if (ts[k1] != NULL && !isDelaunay(ts[i],ts[k1]) && k1 != -1) {
			reconstructTriangle(ts[i],ts[k1]);
		}
		if (ts[k2] != NULL && !isDelaunay(ts[i],ts[k2]) && k2 != -1) {
			reconstructTriangle(ts[i],ts[k2]);
		}
		if (ts[k3] != NULL && !isDelaunay(ts[i],ts[k3]) && k3 != -1) {
			reconstructTriangle(ts[i],ts[k3]);
		}

		// if n1, n2, n3 are already in the list do nothing
		// else insert it
		/*bool result1 = false, result2 = false, result3 = false, result4 = false;
		if (!triangles->isEmpty()) {

		for (int j = 0; j < size; j++)
		{
			if (ts[k1] != NULL && isTheSameTriangle(ts[k1],ts[j])) result1 = true;
			if (ts[k2] != NULL && isTheSameTriangle(ts[k2],ts[j])) result2 = true;
			if (ts[k3] != NULL && isTheSameTriangle(ts[k3],ts[j])) result3 = true;
			if (ts[i] != NULL && isTheSameTriangle(ts[i],ts[j])) result4 = true;
		}
		}
		if (n1 != NULL && result1 == false)	triangles->insert(n1);
		if (n2 != NULL && result2 == false)	triangles->insert(n2);
		if (n3 != NULL && result3 == false)	triangles->insert(n3);
		if (t != NULL && result4 == false)	triangles->insert(t);*/
		triangles->insert(ts[i]);
	}

	// after the while loop, flip is done but the neighbour pointers are not assigned correctly

	// To avoid each triangle might point to its neighbour more than once,
	// set All the neighbour pointers null after flip has been made
	setAllNeighbourPointerNull();

	// reset the neighbour pointers
	resetNeighborPointer();

	// make sure (p1,p2,p3) are in counterclockwise orientation for all the triangles
	turnccw();
}


// check if a triangle is Delaunay with one of its neighbours
bool Triangulator::isDelaunay(Triangle *t, Triangle *neighbor)
{
	if (neighbor == NULL) {
		return true;
	}
	Point* p1 = neighbor->getPoint1();
	Point* p2 = neighbor->getPoint2();
	Point* p3 = neighbor->getPoint3();

	// every triangle shares two points with one of its neighbours
	// only one of (p1,p2,p3) is not a point of the triangle (t)
	// the if clause would determine which of (p1,p2,p3) is not in t
	// let's say p1 is not in t, then isInsideCircumcircle(t,p1) would be executed
	// and a boolean value would be returnd
	if (!isContained(t,p1))	
		return !isInsideCircumcircle(t, p1);
	else if (!isContained(t,p2))
		return !isInsideCircumcircle(t, p2);
	else if (!isContained(t,p3))
		return !isInsideCircumcircle(t, p3);

	// if none of the if and else if statements are executed
	// then t and neighbour are not neighbours
	// for convention, then t and neighbours are Delaunany

	return true;
}

// helper function
// determine would point p is a point of the triangle t
bool Triangulator::isContained(Triangle *t, Point * p)
{
	return (isTheSamePoint(p,t->getPoint1())) || (isTheSamePoint(p,t->getPoint2())) || (isTheSamePoint(p,t->getPoint3()));
}

// post-conditon: return true if p is contained in t 
// return true if p is inside the circumcircle of t
bool Triangulator::isInsideCircumcircle(Triangle *t, Point* p)
{
	// pre-condition: the points in the triangle are in counter-clockwise order
	// with reference to http://en.wikipedia.org/wiki/Delaunay_triangulation
	// determine if point p lies in the circumcircle of triangle t
	// p lies in the circumcircle of t only if the determinant is positive
	int** m = createMatrix(t->getPoint1(),t->getPoint2(),t->getPoint3(),p);
	return det(m) > 0;
}

// helper method: create a 3-3-matrix with four points
// where a,b,c form a triangle
// and d is any other point not inside triangle(a,b,c)
int** Triangulator::createMatrix(Point *a, Point *b, Point *c, Point *d)
{
	// create a 3-by-3 array
	int** m;
	m=new int* [3];
	for(int i=0;i<3;i++)
		*(m+i)=new int[3];

	// construct first column of the matrix
	m[0][0] = a ->getX() - d ->getX();
	m[1][0] = b ->getX() - d ->getX();
	m[2][0] = c ->getX() - d ->getX();

	// construct second column of the matrix
	m[0][1] = a ->getY() - d ->getY();
	m[1][1] = b ->getY() - d ->getY();
	m[2][1] = c ->getY() - d ->getY();

	// construct third column of the matrix
	m[0][2] = (a ->getX())*(a ->getX()) - (d ->getX())*(d ->getX()) + (a ->getY())*(a ->getY()) - (d ->getY())*(d ->getY());
	m[1][2] = (b ->getX())*(b ->getX()) - (d ->getX())*(d ->getX()) + (b ->getY())*(b ->getY()) - (d ->getY())*(d ->getY());
	m[2][2] = (c ->getX())*(c ->getX()) - (d ->getX())*(d ->getX()) + (c ->getY())*(c ->getY()) - (d ->getY())*(d ->getY());
	//int **n = m;
	return m;
}

// compute the determinant
// helper method for computing the determinant
int Triangulator::det (int **m)
{
	//compute the determinant of matrix
	int sum1 = m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1]);
	int sum2 = -m[0][1]*(m[1][0]*m[2][2]-m[2][0]*m[1][2]);
	int sum3 = m[0][2]*(m[1][0]*m[2][1]-m[2][0]*m[1][1]);
	int det = sum1+sum2+sum3;
	return det;
}

// pre-condition: t1 and t2 are neighbour triangles
// delete the line that t1 and t2 share
// with the line deleted, two triangles become a quadrilateral
// connect one of its diagnals of the quadrilateral to produce two new triangles
// post-condtion: the two new triangles created must be different from t1 and t2 (so only 1 choice)
// assign t1 to one the newly created triangles and t2 to the other
// pass t1 and t2 back to flip() (t1 and t2 are passed by reference)

void Triangulator::reconstructTriangle(Triangle* &t1, Triangle* &t2)
{
	Point *p1 = t1->getPoint1();
	Point *p2 = t1->getPoint2();
	Point *p3 = t1->getPoint3();
	Point *p4 = t2->getPoint1();
	Point *p5 = t2->getPoint2();
	Point *p6 = t2->getPoint3();
	
	// two neighbour triangles share two points with a total of 4 points (not duplicated)
	// therefore, there are two points that are not shared
	// sPoint refers to shared points
	// nsPoint refers to not shared points
	Point *nsPoint1 = NULL;
	Point *nsPoint2 = NULL;
	Point *sPoint1 = NULL;
	Point *sPoint2 = NULL;

	// the if statements are used to determine which of (p4,p5,p6) are not in t1
	// let's say p4 is not in t1, then p5 and p6 are in t1
	if (!isContained(t1,p4))	
	{
		nsPoint1 = p4;
		sPoint1 = p5;
		sPoint2 = p6;	
	}
	else if (!isContained(t1,p5))
	{
		nsPoint1 = p5;
		sPoint1 = p4;
		sPoint2 = p6;
	}	
	else if (!isContained(t1,p6))
	{
		nsPoint1 = p6;
		sPoint1 = p4;
		sPoint2 = p5;
	}

	// find the second not shared point

	if (!isContained(t2,p1))	 nsPoint2 = p1;
	else if (!isContained(t2,p2))	 nsPoint2 = p2;
	else if (!isContained(t2,p3))	 nsPoint2 = p3;

	// instead of deleting t1 and t2, and create two new triangles
	// reset the vertex (points) of t1 and t2
	
	t1->setPoint1(nsPoint1);
	t1->setPoint2(nsPoint2);
	t1->setPoint3(sPoint1);
	t2->setPoint1(nsPoint1);
	t2->setPoint2(nsPoint2);
	t2->setPoint3(sPoint2);
}

// Set all the neighbor pointers null
void Triangulator::setAllNeighbourPointerNull()
{
	int i;
	vector <Triangle*> ts = copyTriangles();
	int size = ts.size();
	
	// for each triangle, set all the neighbour pointers null
	for (i = 0; i < size; i++)
	{
		Triangle * t1 = ts[i];
		t1->setNeighbourBetween12(NULL);
		t1->setNeighbourBetween23(NULL);
		t1->setNeighbourBetween31(NULL);
		triangles->insert(t1);
	}
}

// reset all the neighbour pointers
void Triangulator::resetNeighborPointer()
{
    vector <Triangle*> tri = copyTriangles();
    int size = tri.size();

    Triangle *t1, *t2;
    Edge *e12, *e23, *e31, *ee12, *ee23, *ee31;
    for(int i = 0; i < size; i++)
    {
        t1 = tri[i];
        e12 = new Edge(t1->getPoint1(), t1->getPoint2());
        e23 = new Edge(t1->getPoint2(), t1->getPoint3());
        e31 = new Edge(t1->getPoint3(), t1->getPoint1());

        for(int j = 0; j < size; j++)
        {
            if(j != i)
            {
                t2 = tri[j];
                ee12 = new Edge(t2->getPoint1(), t2->getPoint2());
                ee23 = new Edge(t2->getPoint2(), t2->getPoint3());
                ee31 = new Edge(t2->getPoint3(), t2->getPoint3());

                if(e12->equals(ee12) || e12->equals(ee23) || e12->equals(ee31))
                    t1->setNeighbourBetween12(t2);
                if(e23->equals(ee12) || e23->equals(ee23) || e23->equals(ee31))
                    t1->setNeighbourBetween23(t2);
                if(e31->equals(ee12) || e31->equals(ee23) || e31->equals(ee31))
                    t1->setNeighbourBetween31(t2);
            }
        }

        triangles->insert(t1);

    }
		
//	int i =0, j =1;
//	vector <Triangle*> ts = getTriangles();
//	int size = ts.size();
//	for (i = 0; i < size; i++)
//	{
//		Triangle * t1 = ts[i];
//		for (j = 0; j < size; j++)
//		{
//
//		// Do not assign neibhour pointer to itself
//		if (i != j)
//		{
//			// assign t2 as the neighbour of t1 if:
//			//	- t2 shares two points with t1 (see Triangle.h Triangle.cpp)
//			// otherwise, do nothing
//			Triangle *t2 = ts[j];
//			t1->assignNeighbour(t2);
//		}
//		}
//		// each triangle is modified if neccessary and inserted back to the heap queue
//		triangles->insert(t1);
//	}
}

// helper function to determine if p1 and p2 are the same point
bool Triangulator::isTheSamePoint(Point *p1, Point* p2)
{
	return (p1->getX() == p2->getX()) && (p1->getY()==p2->getY());
}

// pre-condition: triangle t1 and t2 are in counter-clockwise orientation
// otherwise, this method would not work
bool Triangulator::isTheSameTriangle(Triangle *t1, Triangle* t2)
{
	// make sure t1 is in counterclockwise orientation
	// if not swap p1, p2 of t1
	if(ccw(t1->getPoint1(),t1->getPoint2(),t1->getPoint3()) == -1)
	{
		Point *temp = t1->getPoint1();
		t1->setPoint1(t1->getPoint2());
		t1->setPoint2(temp);
	}

	// make sure t2 is in counterclockwise orientation
	// if not swap p1, p2 of t2
	if(ccw(t2->getPoint1(),t2->getPoint2(),t2->getPoint3()) == -1)
	{
		Point *temp = t2->getPoint1();
		t2->setPoint1(t2->getPoint2());
		t2->setPoint2(temp);
	}

	Point *p1 = t1 -> getPoint1();
	Point *p2 = t1 -> getPoint2();
	Point *p3 = t1 -> getPoint3();
	Point *p4 = t2 -> getPoint1();
	Point *p5 = t2 -> getPoint2();
	Point *p6 = t2 -> getPoint3();

	// given that (p1,p2,p3) are in counterclockwise orientation
	// given that (p4,p5,p6) are in counterclockwise orientation
	// match (p1,p2,p3) and (p4,p5,p6)
	// if t1 and t2 are really the same triangle
	// then one of the three if statements would be executed and true is returned
	if (isTheSamePoint(p1,p4) && isTheSamePoint(p2,p5) && isTheSamePoint(p3,p6))
		return true;
	else if (isTheSamePoint(p2,p4) && isTheSamePoint(p3,p5) && isTheSamePoint(p1,p6))
		return true;
	else if (isTheSamePoint(p3,p4) && isTheSamePoint(p1,p5) && isTheSamePoint(p2,p6))
		return true;
	else
		return false;
}

vector<Triangle*> Triangulator::copyTriangles() {
	vector<Triangle*>* ts = new vector<Triangle*>(numOfTriangles);
	int i = 0;
	while (!triangles->isEmpty()){
		triangles->deleteMin(ts->at(i));
		if (ts->at(i) != NULL){
			i++;
		}
	}
	ts->resize(i);
	return *ts;	
}
//***************************************************************************************************
